/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 * $Id: EnvironmentMeasurementData.cpp,v 1.3 2009-05-13 07:18:16 amaula Exp $
 *
 * \file
 * \brief Implementation of class CEnvironmentMeasurementData
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 */
#include "EnvironmentMeasurementData.hpp"
#include "owndebug.h"

//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
using namespace MaCI::EnvironmentMeasurement;
//*****************************************************************************
OWNDEBUG_INITIALIZE_MODULENUMBER(); // Initialize module print capability
//*****************************************************************************

CEnvironmentMeasurementData::CEnvironmentMeasurementData()
  : iTimestampPtr(NULL),
    iCommandPtr(NULL),
    iEnvironmentMeasurementPtr(NULL),
    iEnvironmentMeasurementHeaderPtr(NULL),
    iPosePtr(NULL),
    iVariancePtr(NULL),
    iProbabilityPtr(NULL)
{
  SetLastElementType(KTypeUnknown);
}
//*****************************************************************************

CEnvironmentMeasurementData::~CEnvironmentMeasurementData()
{
  // Do any cleanups here.
}
//*****************************************************************************

CEnvironmentMeasurementData::CEnvironmentMeasurementData(const CEnvironmentMeasurementData &aData)
  :  iTimestampPtr(NULL),
     iCommandPtr(NULL),
     iEnvironmentMeasurementPtr(NULL),
     iEnvironmentMeasurementHeaderPtr(NULL),
     iPosePtr(NULL),
     iVariancePtr(NULL),
     iProbabilityPtr(NULL)
{
  // Call assignment operator.
  *this = aData;
}
//*****************************************************************************

CEnvironmentMeasurementData &CEnvironmentMeasurementData::operator=(const CEnvironmentMeasurementData &aData)
{
  // Reset
  Reset();

  // Create an internal BinBag instance
  if (CreateInternalBinBag(aData.iBinBagContainer) == false) {
    dPrint(4,"WARNING: Failed to create internal binbag for object %p from object %p",
           this, &aData);
  }
  
  // Do the decode on the buffer; it sets pointers and ... stuff :)
  if (DecodeFrom(NULL) == false) {
    dPrint(4,"Warning: Failed to Decode CEnvironmentMeasurementData even though assignment operator used?");
  }
  
  return *this; 
}
//*****************************************************************************

bool CEnvironmentMeasurementData::DecodeFrom(gim::binbag::CBinBag *aBinBag)
{
  bool result = true;
  const gim::binbag::TBinBlob *b = NULL;
  int previous_type = -1;
  
  // Store pointer and test it for NullNess. only overwrite if the
  // argument was non-NULL.
  if (aBinBag != NULL) 
    SetBinBagContainerPtr(aBinBag);

  // Parse
  if (iBinBagContainer && iBinBagContainer->Rewind()) {
    while( (b = iBinBagContainer->GetNextBinary()) && result ) {
      switch(b->type) {

      case KTypeTimestamp:
	if (iTimestampPtr == NULL && 
            b->size == sizeof(Common::TTimestamp)) {
	  iTimestampPtr = reinterpret_cast<const Common::TTimestamp *>(b->data);

	} else result = false;
	break;

      case KTypeCommand: {
        if (iCommandPtr == NULL && 
            b->size == sizeof(TCommand)) {
          iCommandPtr = reinterpret_cast<const TCommand *>(b->data); 
          
        } else result = false;
        break;
      }
      case KTypeEnvironmentMeasurementheader:{
        if (iEnvironmentMeasurementHeaderPtr == NULL && 
            b->size == sizeof(TEnvironmentMeasurementHeader)) {

          iEnvironmentMeasurementHeaderPtr = reinterpret_cast<const TEnvironmentMeasurementHeader *>(b->data); 
          
        } else result = false;
        break;

      }
      case KTypeEnvironmentMeasurement:{
        if (iEnvironmentMeasurementPtr == NULL) {

          iEnvironmentMeasurementPtr = b;

        } else result = false;
        
        break;
      }

      case KTypePose2D: {
        if (iPosePtr == NULL && 
            b->size == sizeof(TPose2D)) {
          iPosePtr = reinterpret_cast<const TPose2D *>(b->data); 

        } else result = false;

        break;
      }

      case KTypeVariance2D: {
        if (iVariancePtr == NULL && 
            b->size == sizeof(TVariance2D)) {
          iVariancePtr = reinterpret_cast<const TVariance2D *>(b->data); 

        } else result = false;

        break;
      }
      case KTypeProbability: {
        if (iProbabilityPtr == NULL && 
            b->size == sizeof(TProbability)) {
          iProbabilityPtr = reinterpret_cast<const TProbability *>(b->data); 

        } else result = false;

        break;
      }
      default:
	// Unknown Type in BinBag. Do nothing.
	dPrint(10,"Unknown type %u, ignoring.", b->type);
        break;
      }
      
      // Store previous type
      previous_type = b->type;
    }
  } else result = false;

  // Store the result
  SetReadOnlyContainer(result);

  return result;
}
//*****************************************************************************

void CEnvironmentMeasurementData::Reset()
{
  // Call base class reset.
  CMaCIData::Reset();

  // Clean out. (Same values as in default constructor)
  iTimestampPtr = NULL;
  iCommandPtr = NULL;
#ifndef DISABLE_MACI_ENVIRONMENTMEASUREMENT_EXTERNAL_CONVERSIONS
  iEnvironmentMeasurementPtr = NULL;
#endif
  iEnvironmentMeasurementHeaderPtr = NULL;
  iPosePtr = NULL;
  iVariancePtr = NULL;
  iProbabilityPtr = NULL;
  SetLastElementType(KTypeUnknown);
}
//*****************************************************************************

void CEnvironmentMeasurementData::Print(const int l) const
{
  const gim::binbag::TBinBlob *b = NULL;
  // Parse
  if (iBinBagContainer && iBinBagContainer->Rewind()) {
    while( (b = iBinBagContainer->GetNextBinary())) {
      switch(b->type) {
      case KTypeTimestamp: {
	const Common::TTimestamp *ts = 
          reinterpret_cast<const Common::TTimestamp *>(b->data);
	dPrint(l,"TTimestamp: s=%d, us=%d", 
	       ts->timestamp_s,
	       ts->timestamp_us);
	break;
      }
        
      case KTypeCommand: {
	const TCommand *ts = 
          reinterpret_cast<const TCommand *>(b->data);
	dPrint(l,"TCommand: cmd=%d", 
	       ts->cmd);
	break;
      }
#ifndef DISABLE_MACI_ENVIRONMENTMEASUREMENT_EXTERNAL_CONVERSIONS	
        /*     case KTypeEnvironmentMeasurement: {
	const CEnvironmentMeasurement2D *ae = 
          reinterpret_cast<const CEnvironmentMeasurement2D *>(b->data);
       	dPrint(l,"CEnvironmentMeasurementt: type=%d, data=%d",
               ae->type,
               ae->data);
	break;
      }*/
#endif

      case KTypeEnvironmentMeasurementheader:{
        const TEnvironmentMeasurementHeader *h =
          reinterpret_cast<const TEnvironmentMeasurementHeader *>(b->data);
        dPrint(l,"TEnvironmentMeasurementHeader: EnvironmentMeasurementData contains %u bytes of CEnvironmentMeasurement2D data",
               h->sizeOfData);

        break;
      }
      case KTypePose2D:{
	const TPose2D *p = 
          reinterpret_cast<const TPose2D *>(b->data);
	dPrint(l,"TPose2D: x=%f, y=%f, a=%f",
               p->x,
               p->y,
               p->a);
        break;
      }
      case KTypeVariance2D:{
	const TVariance2D *v = 
          reinterpret_cast<const TVariance2D *>(b->data);
	dPrint(l,"TVariance2D: x=%f, y=%f, a=%f",
               v->x,
               v->y,
               v->a);
        break;
      }             
      case KTypeProbability:{
     	const TProbability *p = 
          reinterpret_cast<const TProbability *>(b->data);
	dPrint(l,"TProbability: probability=%f",
               p->probability);
        break;
      } 

      default:
	// Unknown Type in BinBag. Do nothing.
	dPrint(10,"Unknown type %u, ignoring.", b->type);
        break;
      }
    }
  }
}
//*****************************************************************************

bool CEnvironmentMeasurementData::SetTimestamp(const Common::TTimestamp &aStamp)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iTimestampPtr == NULL) {
    ADDELEMENT(Common::TTimestamp, KTypeTimestamp, iTimestampPtr, aStamp);
    result = true;
  }
  return result;
}
//*****************************************************************************

bool CEnvironmentMeasurementData::SetCommand(const TCommand &aCmd)
{  
  bool result = false;
  if (!IsReadOnlyContainer() && iCommandPtr == NULL) {
    ADDELEMENT(TCommand, KTypeCommand, iCommandPtr, aCmd);
    result = true;
  }
  return result;
}
//*****************************************************************************

bool CEnvironmentMeasurementData::SetPose2D(const TPose2D &aPose)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iPosePtr == NULL) {
    ADDELEMENT(TPose2D, KTypePose2D, iPosePtr, aPose);
    result = true;
  }
  return result;

}
//*****************************************************************************
     
bool CEnvironmentMeasurementData::SetVariance2D(const TVariance2D &aVar)
{
bool result = false;
  if (!IsReadOnlyContainer() && iVariancePtr == NULL) {
    ADDELEMENT(TVariance2D, KTypeVariance2D, iVariancePtr, aVar);
    result = true;
  }
  return result;

}
//*****************************************************************************    
 
bool CEnvironmentMeasurementData::SetProbability(const TProbability &aProb)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iProbabilityPtr == NULL) {
    ADDELEMENT(TProbability, KTypeProbability, iProbabilityPtr, aProb);
    result = true;
  }
  return result;


}
//*****************************************************************************
//*****************************************************************************
#ifndef DISABLE_MACI_ENVIRONMENTMEASUREMENT_EXTERNAL_CONVERSIONS
     
bool CEnvironmentMeasurementData::SetEnvironmentMeasurement(CEnvironmentMeasurement2D *aEMeasurement2D)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iEnvironmentMeasurementPtr == NULL && iEnvironmentMeasurementHeaderPtr ==NULL) {
    char *data = NULL;
    int bytes = aEMeasurement2D->getNetPacket(&data);
    TEnvironmentMeasurementHeader header((unsigned int)bytes);
    //First add the header
    ADDELEMENT(TEnvironmentMeasurementHeader, KTypeEnvironmentMeasurementheader, iEnvironmentMeasurementHeaderPtr, header);

    
    iEnvironmentMeasurementPtr =iBinBagContainer->AddBinary(KTypeEnvironmentMeasurement,
                                                             (const char *)data,
                                                             bytes);

    SetLastElementType(KTypeEnvironmentMeasurement);

    result = true;
  }else{
    dPrint(5,"Failed to set Environment measurement data");
  }
  return result;

}
//*****************************************************************************
     
bool CEnvironmentMeasurementData::GetEnvironmentMeasurement(CEnvironmentMeasurement2D *aEMeasurement2D) const
{

  bool result = false;
  if(iEnvironmentMeasurementPtr != NULL && iEnvironmentMeasurementPtr->type == KTypeEnvironmentMeasurement &&
     iEnvironmentMeasurementHeaderPtr != NULL){

    result = aEMeasurement2D->setNetPacket(iEnvironmentMeasurementPtr->data,
                                           iEnvironmentMeasurementHeaderPtr->sizeOfData);
   

  }else{ 
    dPrint(5,"No Environment measurement data found");
    if(iEnvironmentMeasurementPtr == NULL){
      dPrint(8,"iEnvironmentMeasurementPtr == NULL");
    
    }else if(iEnvironmentMeasurementPtr->type != KTypeEnvironmentMeasurement){
      dPrint(8,"iEnvironmentMeasurementPtr->type != KTypeEnvironmentMeasurement");
    }
    

    if( iEnvironmentMeasurementHeaderPtr == NULL){
      dPrint(8,"EnvironmentMeasurementHeaderPtr == NULL");
    }
  }
  return result; 

}
#endif
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

const MaCI::Common::TTimestamp *CEnvironmentMeasurementData::GetTimestamp(void) const
{
  return iTimestampPtr;
}
//*****************************************************************************

const TCommand *CEnvironmentMeasurementData::GetCommand(void) const
{
  return iCommandPtr;
}
//*****************************************************************************

const TPose2D *CEnvironmentMeasurementData::GetPose2D(void) const
{
  return iPosePtr;

}
//*****************************************************************************

const TVariance2D *CEnvironmentMeasurementData::GetVariance2D(void) const
{
  return iVariancePtr;
}
//*****************************************************************************

const TProbability *CEnvironmentMeasurementData::GetProbability(void) const
{

  return iProbabilityPtr;
}

//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

#ifdef COMPILE_ENVIRONMENTMEASUREMENTDATA_STANDALONE_MAIN
#include <string>

int main(void)
{
  using namespace MaCI::EnvironmentMeasurement;
  gim::binbag::CBinBag bb;
  CEnvironmentMeasurementData A;
  CEnvironmentMeasurementData B;
  CEnvironmentMeasurement2D em2d;
   bool r;

  printf("Carry out Encoding/Decoding test.\n");

  printf("Encoding...\n");
  // Do Encoding 
  A.EncodeTo(&bb);
  
  printf("Encode: Timestamp(1,2) ... ");
  r = A.SetTimestamp(MaCI::Common::TTimestamp(1, 2));
  assert(r == true);
  printf("OK.\n");

  printf("Encode: TPose2D(1.0, 2.0, 3.0)...");
  A.SetPose2D(TPose2D(1.0, 2.0, 3.0));
  printf("OK.\n");

  printf("Encode: TVariance2D(0.1, 0.2, 0.3)...");
  A.SetVariance2D(TVariance2D(0.1, 0.2, 0.3));
  printf("OK.\n");

  printf("Encode: TProbability(0.1234)...");
  A.SetProbability(TProbability(0.1234));
  printf("OK.\n");

  printf("Encode: CEnvironmentMeasurement2D()");
  if(A.SetEnvironmentMeasurement(&em2d)){
    printf("OK.\n");
  }else{
    printf("FAILED!\n");
    exit(1);
  }

 
  /**
   * \todo Add test sections for TEnvironmentMeasurementEvent and TCommand
   */
  //////////////////////////////////////////////////////////
  printf("Decoding...\n");
  // Do Decoding
  B.DecodeFrom(&bb);
  printf("Decode: GetTimestamp() = (%d, %d)\n", 
	 B.GetTimestamp()->timestamp_s, 
	 B.GetTimestamp()->timestamp_us);
  assert(B.GetTimestamp()->timestamp_s == 1);
  assert(B.GetTimestamp()->timestamp_us == 2);

  printf("Decode: TPose2D = (%.1f, %.1f, %.1f)\n", 
	 B.GetPose2D()->x, 
	 B.GetPose2D()->y, 
	 B.GetPose2D()->a);

  printf("Decode: TVariance2D = (%.1f, %.1f, %.1f)\n", 
	 B.GetVariance2D()->x, 
	 B.GetVariance2D()->y, 
	 B.GetVariance2D()->a);

  printf("Decode: TProbability = (%.4f)\n", 
         B.GetProbability()->probability);

  CEnvironmentMeasurement2D em2D_2;
  if(B.GetEnvironmentMeasurement(&em2D_2)){
    printf("Decoding of EnvironmentMeasurement succeess");
  }else{
    printf("Decoding of EnvironmentMeasurement failed!");
    exit(1);
  }

  printf("Test complete.\n");
  

}
#endif
//*****************************************************************************
//*****************************************************************************
